/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "crc.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stm32g4xx_hal.h"
#include <stdio.h>
#include <string.h>
#include "mypwm.h"
#include "myhall.h"
#include "mysystem.h"
#include "my485.h"
#include "mymodbus.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
uint8_t debugval = 0;
uint16_t modbusdebug = 0;
uint32_t debugnum = 0;

uint8_t debugbuf[256];
char usart1_recvBuff[usart1_buffersize];

uint32_t system_time_s;
char ModBus_sendBuff[32];
volatile uint8_t recvLength = 0;  //接收一帧数据的长度
volatile uint8_t recvEndFlag = 0; //一帧数据接收完成标志

uint8_t usart1_tempbuf;
uint8_t usart3_tempbuf;
uint8_t usart3_Rx_Cnt = 0;

bool LeftButton_state = 0;

char usart3_RxBuffer[8];

uint16_t CurrentADC_Value;

uint8_t	motorStateFLAG = MOTOR_IDLE;
uint8_t motorStartFlAG = 0;

volatile TimeFLAG_Typedef TimeVars = Time_VARS_INIT;
volatile MotorVars_t Motor_Vars = MOTOR_VARS_INIT;
volatile Foot_Typedef Foot_Vars = FOOT_VARS_INIT;
volatile Hand_Typedef Hand_Vars = HAND_VARS_INIT;
volatile PID_Typedef Speed_PI = CloseSpeedPI_VARS_INIT;
volatile PID_Typedef Speed1_PI = CloseSpeed1PI_VARS_INIT;
volatile PID_Typedef Position_PI = ClosePositionPI_VARS_INIT;
volatile PID_Typedef Position1_PI = ClosePosition1PI_VARS_INIT;
//volatile PID_Typedef Current_PI = CloseCurrentPI_VARS_INIT;
//volatile PID_Typedef Current1_PI = CloseCurrent1PI_VARS_INIT;

uint16_t TIM1_DMA_Buffer[TIM1_DMA_BufferSize];


uint16_t My_adcData[adc_max];

uint8_t DMA_TxDone=1;
uint32_t hall_countAdd;
uint32_t hall_countSubtract;
extern uint8_t hall_countSubtract_flag;
extern uint8_t hall_countAdd_flag;
bool adc_data_ready = 0;
bool commucation_flag = false;
uint32_t test_count=0;
bool pretarget_finish_flag = false;

#define DMA_TIMEOUT_THRESHOLD 4000  // 50us * 4000 = 200ms

volatile bool dma_updated = false;

#define target_speed 40000.0f
#define acc_time 10.0f*1e-3
#define acc_cycle 1.0f*1e-3
#define EPSILON 1e-3



/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

void SystemClock_Config(void);
extern void UART_Print_MotorSpeed(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main( )
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
	HAL_Delay(100);
	__HAL_DBGMCU_FREEZE_TIM2(); 
	__HAL_DBGMCU_FREEZE_TIM1();
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART3_UART_Init();
  MX_TIM7_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM6_Init();
  MX_ADC1_Init();
  MX_USART1_UART_Init();
  MX_CRC_Init();
  MX_TIM3_Init();
  MX_I2C1_Init();
  /* USER CODE BEGIN 2 */
	HAL_GPIO_WritePin(BREAK_PVDD_GPIO_Port,BREAK_PVDD_Pin,GPIO_PIN_SET);
	HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED); 
	HAL_ADC_Start_DMA(&hadc1, (uint32_t *)My_adcData, adc_max);
	 __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE); //使能IDLE中断
	HAL_UART_Receive_DMA(&huart1, (uint8_t *)usart1_recvBuff, usart1_buffersize);
	
	HAL_UART_Receive_IT(&huart3, (uint8_t *)&usart3_tempbuf, 1);
	
	
	HAL_TIM_Base_Start(&htim6);
	HAL_TIM_Base_Start_IT(&htim3);
	HAL_TIM_Base_Start_IT(&htim7);
	HAL_TIM_Base_Start_IT(&htim1);
//	HAL_TIM_OC_Start_IT(&htim1,TIM_CHANNEL_4);
	
	HAL_TIMEx_ConfigCommutEvent(&htim1, TIM_TS_ITR1, TIM_COMMUTATION_TRGI);
	
	HAL_DMA_Start_IT(&hdma_tim1_com, (uint32_t)&TIM6->CNT, (uint32_t)TIM1_DMA_Buffer,TIM1_DMA_BufferSize);
	__HAL_TIM_ENABLE_DMA(&htim1, TIM_DMA_COM);
	
	

	Modbus_Init();
//	__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_COM);
//	HAL_TIMEx_HallSensor_Start(&htim2);					  	//开启接口定时器

	HAL_Delay(1000);
//	Motor_Vars.motorStartFlAG = 1;
	Motor_Start_Task();


//	TIM6->CNT = 0;

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

//	uint32_t start1 = HAL_GetTick();
//	uint8_t count1 = 0;
//  Theta_Accumulate = 72000.0;
//  actual_postion = 0.0;
//  uint8_t count2=0;
//  uint32_t temp_duty = 200;
//
  bool fisrt_in=false;
  HAL_Delay(10);
  Motor_Vars.motorStateFLAG = MOTOR_RUN;
  uint16_t count5=0;

  while (1)
  {
	  TaskManager();

//	  if(motor_state==START)
//	  {
//		  count5++;
//		  if(count5==1)
//			  break;
//	  }

//	  if(!fisrt_in)
//	  {
//		  pretarget_finish_flag = true;
//		  Motor_Start_Task();
////		  HAL_Delay(1);
//		  Motor_Vars.motorStateFLAG = MOTOR_RUN;
//		  fisrt_in = true;
//	  }
//
//	  if(fabs(72000.0f-accelerate_count*60.0f)<=180.0f)
//	  {
//		 Motor_Vars.motorStateFLAG = MOTOR_IDLE;
////		 __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_COM);
////		 Fast_Stop_Motor(5665);
////		 HAL_Delay(1);
//		 __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_COM);
//		 Motor_Vars.Motor_ActuralDuty = 200;
//		 pretarget_finish_flag = false;
//		 Speed_PI.I = 0;
//	  }


//	Motor_Vars.Motor_ActuralSpeed = Hall_SpeedMonitor_RPM(TIM1_DMA_Buffer, TIM1_DMA_BufferSize);
//	if((HAL_GetTick()-start1)>2000)
//	{
//		start1 = HAL_GetTick();
//		if(count1%2==0)
//		{
//			Motor_Vars.motorStateFLAG = MOTOR_IDLE;
////			Motor_Vars.Motor_ActuralDuty = 0.0f;
//			__HAL_TIM_DISABLE_IT(&htim1, TIM_IT_COM);
//			Fast_Stop_Motor(5665);
//			HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
//			Speed_PI.I = 0;
//		}
//		else
//		{
//			Motor_Vars.Motor_DIR^=0x01;
//			Motor_Start_Task();
//			Motor_Vars.motorStateFLAG = MOTOR_RUN;
//			HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
//			Speed_PI.I = 0;
//		}
//		count1++;
//
//	}
//		HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
//		HAL_Delay(200);

    /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */
}



/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
  RCC_OscInitStruct.PLL.PLLN = 85;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }
}

static int8_t update_speed_dir(uint8_t dir_in)
{
  uint8_t step[6] = {1, 3, 2, 6, 4, 5};

  static uint8_t num_old = 0;
  uint8_t step_loc = 0;    // 记录当前霍尔位置
  int8_t dir = 1;

  for (step_loc=0; step_loc<6; step_loc++)
  {
    if (step[step_loc] == dir_in)    // 找到当前霍尔的位置
    {
      break;
    }
  }

  /* 端点处理 */
  if (step_loc == 0)
  {
    if (num_old == 1)
    {
      dir = 1;
    }
    else if (num_old == 5)
    {
      dir = -1;
    }
  }
  /* 端点处理 */
  else if (step_loc == 5)
  {
    if (num_old == 0)
    {
      dir = 1;
    }
    else if (num_old == 4)
    {
      dir = -1;
    }
  }
  else if (step_loc > num_old)
  {
    dir = -1;
  }
  else if (step_loc < num_old)
  {
    dir = 1;
  }

  num_old = step_loc;
  return dir;
}

void pretargeting(uint8_t* temp_hall,uint8_t target_hall,bool finish_flag)
{
	static bool pretarget_flag = false;

	if (!finish_flag)
	{
		if(*temp_hall == target_hall)
		{
			pretarget_flag = true;
		}

		if(pretarget_flag)
		{
			*temp_hall = target_hall;
		}
	}
	else
	{
		pretarget_flag = false;
	}
}

/* USER CODE BEGIN 4 */

void HAL_TIMEx_CommutCallback(TIM_HandleTypeDef *htim)
{
  /* Prevent unused argument(s) compilation warning */
  UNUSED(htim);

  TIM6->CNT = 0;


  if (htim->Instance == TIM1) {
		if(Motor_Vars.Motor_FastStopFlag)
		{
			Fast_Stop_Motor(FastStopSpeed);
		}
		else
		{
			uint8_t temp = Hall_Get_State();

//			pretargeting(&temp, 1, pretarget_finish_flag);


			if(motor_state == REVERSE_DECEL)
			{
				pretargeting(&temp, 3, pretarget_finish_flag);
			}
			else
			{
				pretargeting(&temp, 1, pretarget_finish_flag);
			}

			UpdateMos_CW(temp, fabs(Motor_Vars.Motor_ActuralDuty),Motor_Vars.Motor_DIR);

			if(Motor_Vars.Motor_DIR == 0)
			{
				if(update_speed_dir(temp)==-1)
					accelerate_count++;
				else
					accelerate_count--;
//					if(Motor_Vars.Motor_ActuralDuty>0)
//					    UpdateMos_CW(temp, fabs(Motor_Vars.Motor_ActuralDuty));
//					else
//						UpdateMos_CCW(temp, fabs(Motor_Vars.Motor_ActuralDuty));
			}
			else
			{
				if(update_speed_dir(temp)==1)
				{
					accelerate_count++;
				}
				else
				{
					accelerate_count--;
				}

//					if(Motor_Vars.Motor_ActuralDuty>0)
//					{
//						UpdateMos_CW(temp, fabs(Motor_Vars.Motor_ActuralDuty));
//					}
//					else
//					{
//						UpdateMos_CW(temp, fabs(Motor_Vars.Motor_ActuralDuty));
//						test_count++;
//					}

			}

	  }


    }
	
  /* NOTE : This function should not be modified, when the callback is needed,
            the HAL_TIMEx_CommutCallback could be implemented in the user file
   */
}

/*
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim) 
	{
		if (htim->Instance == TIM1 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
			{
				HAL_ADC_Start_DMA(&hadc1, (uint32_t *)My_adcData, adc_max);
			}
}
*/

//void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {
//    if (hadc->Instance == ADC1) {
//    	adc_data_ready = 1;
//    }
//}
// a

void SettingSpeedCurve(MotorState_t state)
{
	float acc_speed = target_speed / acc_time;
	float speed_delta = acc_speed * acc_cycle;
	static float speed_temp = 0.0f;
	if((state == FORWARD)||(state == REVERSE_ACCEL))
	{
		if(fabs(speed_temp - target_speed) > EPSILON)
		{
			speed_temp += speed_delta;
			Position_PI.Umax = speed_temp;
		}
	}
	if((state == DECELERATE)||(state == REVERSE_DECEL))
		speed_temp = 0.0f;
}


void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if (htim == (&htim1))
	{
//		if((TIM1->CR1 & TIM_CR1_DIR) == 0)
//		{
//			HAL_ADC_Start_DMA(&hadc1, (uint32_t *)My_adcData, adc_max);
//		}
//		debugnum = TIM1->CNT;
//		if(TIM1->CNT > 350)
//		{
//			HAL_ADC_Start_DMA(&hadc1, (uint32_t *)My_adcData, adc_max);
//			debugnum = My_adcData[0];
//			
//		}
	}
	if (htim == (&htim7))//50us
	{
		TimeVars.periodFLAG_50us = 1;
		

		if(TimeVars.periodCount_200ms)
		{
			TimeVars.periodCount_200ms--;
			if(TimeVars.periodCount_200ms == 0)
			{
				TimeVars.periodFLAG_200ms = 1;
				TimeVars.periodCount_200ms = 4000;
			}
		}
		if(TimeVars.periodCount_500ms)
		{
			TimeVars.periodCount_500ms--;
			if(TimeVars.periodCount_500ms == 0)
			{
				TimeVars.periodFLAG_500ms = 1;
				TimeVars.periodCount_500ms = 10000;
			}
		}
		if(TimeVars.periodCount_1ms)
		{
			TimeVars.periodCount_1ms--;
			if(TimeVars.periodCount_1ms == 0)
			{
				TimeVars.periodFLAG_1ms = 1;
				TimeVars.periodCount_1ms = 20;
//				SettingSpeedCurve(motor_state);
			}
		}
		if(TimeVars.periodCount_10ms)
		{
			TimeVars.periodCount_10ms--;
			if(TimeVars.periodCount_10ms == 0)
			{
				TimeVars.periodFLAG_10ms = 1;
				TimeVars.periodCount_10ms = 200;
			}
		}
		if(TimeVars.periodCount_1s)
		{
			TimeVars.periodCount_1s--;
			if(TimeVars.periodCount_1s == 0)
			{
				TimeVars.periodFLAG_1s = 1;
				TimeVars.periodCount_1s = 20000;
			}
		}
	}
}

void HAL_ADC_LevelOutOfWindowCallback(ADC_HandleTypeDef *hadc)
{
	if (hadc->Instance == ADC1)
	{

			
			if(LeftButton_state == 0)
			{
				if(Motor_Vars.ControlMode == 2)
				{
					if(Motor_Vars.Motor_spinFlag)
					{
						Motor_Vars.Motor_FastStopFlag = 1;
						Motor_Vars.Foot_ButtonErrorFlag = 1;
					}
						Foot_Vars.foot_dir = !Foot_Vars.foot_dir;
						Motor_Vars.Motor_DIR = Foot_Vars.foot_dir;
				}
				LeftButton_state = 1;
			}
	}
}



void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart == &huart1)
    {
        HAL_GPIO_WritePin(RS485_RE_GPIO_Port, RS485_RE_Pin, GPIO_PIN_RESET);
    }
    if(huart == &huart3)
    {
    	DMA_TxDone=1;
    }
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	
	/* Prevent unused argument(s) compilation warning */
  UNUSED(huart);
  /* NOTE: This function Should not be modified, when the callback is needed,
           the HAL_UART_TxCpltCallback could be implemented in the user file
  */
//	if(huart == (&huart1))
//	{
//		debugval++;
//	}
	if(huart == (&huart3))
	{
			if(usart3_Rx_Cnt > 6)
	{
		usart3_RxBuffer[usart3_Rx_Cnt] = usart3_tempbuf;
		while(HAL_UART_GetState(&huart3) == HAL_UART_STATE_BUSY_TX);
		
		if((usart3_RxBuffer[6] == 0x0A) && (usart3_RxBuffer[7] == 0x0D))
		{
			if(usart3_RxBuffer[0] == 0)
			{
				Motor_Vars.Motor_DIR = 0;
			}
			else
			{
				Motor_Vars.Motor_DIR = 1;
			}
			
			Motor_Vars.Motor_ActuralDuty = usart3_RxBuffer[1];
			uint32_t temp = usart3_RxBuffer[2] << 16 | usart3_RxBuffer[3] << 8 | usart3_RxBuffer[4];
			Motor_Vars.Motor_AimSpeed = temp;
			
			if(usart3_RxBuffer[5] == 0)
			{
				Motor_Vars.motorStopFlAG = 1;
			}
			if(usart3_RxBuffer[5] == 1)
			{
				Motor_Vars.motorStartFlAG = 1;
			}
		}
		
		usart3_Rx_Cnt = 0;
		memset(usart3_RxBuffer, 0, sizeof(usart3_RxBuffer));
	}
	else{
		usart3_RxBuffer[usart3_Rx_Cnt++] = usart3_tempbuf;
	}
	HAL_UART_Receive_IT(&huart3, (uint8_t *)&usart3_tempbuf, 1);
	}

}


/**
  * 函数功能: 重定向c库函数printf到DEBUG_USARTx
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：无
  */
int fputc(int ch, FILE *f)
{
  HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 0xffff);
  return ch;
}
 
/**
  * 函数功能: 重定向c库函数getchar,scanf到DEBUG_USARTx
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：无
  */
int fgetc(FILE *f)
{
  uint8_t ch = 0;
  HAL_UART_Receive(&huart3, &ch, 1, 0xffff);
  return ch;
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
